//
//  ContentView.swift
//  Seove
//
//  Created by liyahao on 2025/5/26.
//

import SwiftUI
import CoreMotion
import Network // For TCP communication

// MARK: - Motion Manager
class MotionManager: ObservableObject {
    private let motionManager = CMMotionManager()
    
    @Published var verticalAngle: Double = 90.0
    @Published var horizontalAngle: Double = 90.0
    @Published var flipAngle: Double = 90.0

    private var pitchOffset: Double = 0.0
    private var rollOffset: Double = 0.0
    private var yawOffset: Double = 0.0
    private var isCalibrated: Bool = false

    init() {
        if motionManager.isDeviceMotionAvailable {
            motionManager.deviceMotionUpdateInterval = 1.0 / 30.0 // 30 Hz is usually enough
            motionManager.showsDeviceMovementDisplay = true
        } else {
            print("Device Motion not available")
        }
    }

    func startUpdates() {
        guard motionManager.isDeviceMotionAvailable else { return }
        
        // Reset offsets before starting
        // For a more robust calibration, you might want a specific user action (e.g., button press)
        // This simple calibration takes the initial orientation as the zero point.
        if let data = motionManager.deviceMotion {
             pitchOffset = data.attitude.pitch
             rollOffset = data.attitude.roll
             yawOffset = data.attitude.yaw
             isCalibrated = true
             print("Calibrated with initial offsets: P:\(pitchOffset), R:\(rollOffset), Y:\(yawOffset)")
        } else {
            // If no initial data, start and calibrate on first data point
            isCalibrated = false
        }

        motionManager.startDeviceMotionUpdates(using: .xArbitraryCorrectedZVertical, to: .main) { [weak self] (data, error) in
            guard let self = self, let motion = data else {
                if let error = error {
                    print("Motion update error: \(error.localizedDescription)")
                }
                return
            }

            if !self.isCalibrated {
                self.pitchOffset = motion.attitude.pitch
                self.rollOffset = motion.attitude.roll
                self.yawOffset = motion.attitude.yaw
                self.isCalibrated = true
                print("Calibrated with first data: P:\(self.pitchOffset), R:\(self.rollOffset), Y:\(self.yawOffset)")
            }

            // Calculate relative angles from the offset
            let relativePitch = motion.attitude.pitch - self.pitchOffset
            let relativeRoll = motion.attitude.roll - self.rollOffset
            let relativeYaw = motion.attitude.yaw - self.yawOffset
            
            // Convert radians to degrees
            let pitchDegrees = relativePitch * 180.0 / .pi
            let rollDegrees = relativeRoll * 180.0 / .pi
            let yawDegrees = relativeYaw * 180.0 / .pi

            // Map to 0-180 for servos.
            // Pitch (-90 to +90) -> Vertical (0 to 180)
            self.verticalAngle = self.clampAngle(pitchDegrees + 90.0)
            
            // Roll (-90 to +90, sometimes -180 to +180 depending on orientation) -> Horizontal (0 to 180)
            // Assuming roll is within +/- 90 for typical holding.
            self.horizontalAngle = self.clampAngle(rollDegrees + 90.0)
            
            // Yaw (-180 to +180) -> Flip (0 to 180)
            // This mapping might need significant adjustment based on desired "flip" behavior.
            // Simple mapping: (yaw + 180) / 2 maps -180..180 to 0..180
            // Or map a smaller range of yaw, e.g. -90 to +90 of yaw: (yawDegrees + 90.0)
            self.flipAngle = self.clampAngle((yawDegrees + 180.0).truncatingRemainder(dividingBy: 360.0) * (180.0 / 360.0)) // Example mapping

            // Alternative simple yaw mapping to 0-180 (adjust as needed)
            // self.flipAngle = self.clampAngle(yawDegrees + 90.0) // if only -90 to +90 yaw is relevant
        }
    }

    private func clampAngle(_ angle: Double) -> Double {
        return max(0, min(180, angle))
    }

    func stopUpdates() {
        if motionManager.isDeviceMotionAvailable {
            motionManager.stopDeviceMotionUpdates()
        }
    }
}

// MARK: - Network Manager
class NetworkManager: ObservableObject {
    var connection: NWConnection?
    let queue = DispatchQueue(label: "TCPClientQueue", qos: .userInitiated)
    @Published var isConnected: Bool = false
    @Published var connectionStatusMessage: String = "Disconnected"

    func connect(host: String, port: UInt16) {
        if connection != nil {
            disconnect() // Disconnect if already connected or attempting
        }
        
        let hostTCP = NWEndpoint.Host(host)
        guard let portTCP = NWEndpoint.Port(rawValue: port) else {
            self.connectionStatusMessage = "Invalid Port"
            return
        }

        connection = NWConnection(host: hostTCP, port: portTCP, using: .tcp)
        connection?.stateUpdateHandler = { [weak self] (newState) in
            DispatchQueue.main.async {
                guard let self = self else { return }
                switch newState {
                case .ready:
                    self.isConnected = true
                    self.connectionStatusMessage = "Connected to \(host):\(port)"
                    print("NetworkManager: Connected!")
                case .failed(let error):
                    self.isConnected = false
                    self.connectionStatusMessage = "Connection failed: \(error.localizedDescription)"
                    print("NetworkManager: Connection failed: \(error.localizedDescription)")
                    self.connection = nil
                case .cancelled:
                    self.isConnected = false
                    self.connectionStatusMessage = "Disconnected"
                    print("NetworkManager: Connection cancelled.")
                    self.connection = nil
                case .waiting(let error):
                    self.isConnected = false
                    self.connectionStatusMessage = "Waiting for network: \(error.localizedDescription)"
                     print("NetworkManager: Waiting for network: \(error.localizedDescription)")
                case .preparing:
                    self.connectionStatusMessage = "Preparing connection..."
                    print("NetworkManager: Preparing connection...")
                default:
                    break
                }
            }
        }
        connection?.start(queue: queue)
    }

    func send(angles: [String: Double]) {
        guard isConnected, let connection = connection else {
            print("NetworkManager: Not connected or connection is nil.")
            // Optionally try to reconnect or notify user
            return
        }

        do {
            let jsonData = try JSONEncoder().encode(angles)
            // ESP32 expects a newline terminated string for simpler parsing
            guard var jsonString = String(data: jsonData, encoding: .utf8) else {
                print("NetworkManager: Failed to convert JSON data to string.")
                return
            }
            jsonString += "\n"
            
            let dataToSend = Data(jsonString.utf8)
            
            connection.send(content: dataToSend, completion: .contentProcessed({ (error) in
                DispatchQueue.main.async {
                    if let error = error {
                        print("NetworkManager: Send error: \(error.localizedDescription)")
                        // Handle error, e.g., by attempting to reconnect or informing user
                        // self.disconnect() // Or try to handle more gracefully
                    } else {
                        // print("NetworkManager: Data sent successfully: \(jsonString.trimmingCharacters(in: .newlines))")
                    }
                }
            }))
        } catch {
            print("NetworkManager: JSON Encoding error: \(error.localizedDescription)")
        }
    }

    func disconnect() {
        connection?.cancel()
        // connection = nil // stateUpdateHandler will set it to nil on .cancelled
    }
}


// MARK: - Main Content View
struct ContentView: View {
    @StateObject private var motionManager = MotionManager()
    @StateObject private var networkManager = NetworkManager()

    @State private var manualVertical: Double = 90.0
    @State private var manualHorizontal: Double = 90.0
    @State private var manualFlip: Double = 90.0
    @State private var isManualMode: Bool = true // Start in manual mode for safety

    // ESP32 IP Address and Port
    @State private var esp32Host: String = "192.168.4.1" // Default AP IP for ESP32
    @State private var esp32Port: String = "8888"

    // Timer for sending data periodically in gyro mode
    let gyroSendTimer = Timer.publish(every: 0.1, on: .main, in: .common).autoconnect() // Send 10 times per second

    var body: some View {
        NavigationView {
            Form {
                Section(header: Text("Connection")) {
                    HStack {
                        Text("ESP32 IP:")
                        TextField("e.g., 192.168.4.1", text: $esp32Host)
                            .keyboardType(.decimalPad)
                    }
                    HStack {
                        Text("Port:")
                        TextField("e.g., 8888", text: $esp32Port)
                            .keyboardType(.numberPad)
                    }
                    
                    Button(networkManager.isConnected ? "Disconnect" : "Connect") {
                        if networkManager.isConnected {
                            networkManager.disconnect()
                        } else {
                            guard let port = UInt16(esp32Port) else {
                                networkManager.connectionStatusMessage = "Invalid Port Number"
                                return
                            }
                            networkManager.connect(host: esp32Host, port: port)
                        }
                    }
                    Text(networkManager.connectionStatusMessage)
                        .font(.caption)
                        .foregroundColor(networkManager.isConnected ? .green : .red)
                }

                Section(header: Text("Control Mode")) {
                    Toggle(isOn: $isManualMode.animation()) {
                        Text("Manual Control Mode")
                    }
                }

                if isManualMode {
                    Section(header: Text("Manual Angles (0-180°)")) {
                        AngleSliderView(label: "Vertical", angle: $manualVertical, onEditingChanged: sendManualData)
                        AngleSliderView(label: "Horizontal", angle: $manualHorizontal, onEditingChanged: sendManualData)
                        AngleSliderView(label: "Flip", angle: $manualFlip, onEditingChanged: sendManualData)
                    }
                } else {
                    
                    Section(header: Text("Gyroscope Angles (Live)")) {
                        Text("Vertical: \(motionManager.verticalAngle, specifier: "%.1f")°")
                        Text("Horizontal: \(motionManager.horizontalAngle, specifier: "%.1f")°")
                        Text("Flip: \(motionManager.flipAngle, specifier: "%.1f")°")
                        
                        Button("Recalibrate Gyro Zero Point") {
                            motionManager.stopUpdates() // Stop briefly
                            motionManager.startUpdates() // Restart to recalibrate
                        }
                    }
                }
            }
            .navigationTitle("ESP32 Servo Control")
            .onAppear {
                if !isManualMode {
                    motionManager.startUpdates()
                }
            }
            .onDisappear {
                motionManager.stopUpdates()
                networkManager.disconnect()
            }
            .onChange(of: isManualMode) { newMode in
                if newMode { // Switched to Manual
                    motionManager.stopUpdates()
                    // Send current manual values once when switching to manual
                    sendManualData()
                } else { // Switched to Gyro
                    motionManager.startUpdates()
                    // Gyro data will be sent by the timer or onChange listeners
                }
            }
            .onReceive(gyroSendTimer) { _ in
                if !isManualMode && networkManager.isConnected {
                    sendGyroData()
                }
            }
        }
    }

    func sendManualData() {
        guard networkManager.isConnected else { return }
        let angles: [String: Double] = [
            "vertical": manualVertical,
            "horizontal": manualHorizontal,
            "flip": manualFlip
        ]
        networkManager.send(angles: angles)
    }
    
    func sendGyroData() {
        guard networkManager.isConnected else { return }
        let angles: [String: Double] = [
            "vertical": motionManager.verticalAngle,
            "horizontal": motionManager.horizontalAngle,
            "flip": motionManager.flipAngle
        ]
        networkManager.send(angles: angles)
    }
}

// MARK: - Helper Angle Slider View
struct AngleSliderView: View {
    var label: String
    @Binding var angle: Double
    var onEditingChanged: () -> Void // Closure to call when slider editing finishes

    var body: some View {
        VStack(alignment: .leading) {
            Text("\(label): \(angle, specifier: "%.0f")°")
            Slider(value: $angle, in: 0...180, step: 1.0) { editing in
                if !editing { // User has finished dragging the slider
                    onEditingChanged()
                }
            }
        }
    }
}

// MARK: - App Entry Point
// In your YourAppNameApp.swift file:
// @main
// struct YourAppNameApp: App {
//     var body: some Scene {
//         WindowGroup {
//             ContentView()
//         }
//     }
// }

#Preview {
    ContentView()
}
